{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 第一次课程作业——摄像机标定"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 任务  \n",
    "\n",
    "- 通过3D摄像机标定物的单张图像进行摄像机标定\n",
    "- 以 imgages 文件夹中的 **Jietu20200301-091513.jpg** 图片为例，该示例中选择了三个平面的交线作为世界坐标系的 x、y 和 z 轴，其中共选择了 12 个点（三个四边形的顶点）。\n",
    "- 通过选定的点进行相机标定，算出 M、K、R、T。\n",
    "- 验证结果，从世界坐标系中引入一些点，并检查它们是否可以通过 M 映射到相应的像素。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<div align=center><img src=\"images/jietu20200301-091513.jpg\" width=500></div> "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 示例代码"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "class SingleCamera:\n",
    "\n",
    "    def __init__(self, world_coor, pixel_coor, n):\n",
    "\n",
    "        self.__world_coor = world_coor\n",
    "        self.__pixel_coor = pixel_coor\n",
    "        self.__point_num = n\n",
    "\n",
    "        '''\n",
    "        0. P is the appropriate form when Pm=0\n",
    "        1. SVD-solved M is known up to scale, \n",
    "        which means that the true values of the camera matrix are some scalar multiple of M,\n",
    "        recorded as __roM\n",
    "        2. __M can be represented as form [A b], where A is a 3x3 matrix and b is with shape 3x1\n",
    "        3. __K is the intrisic Camera Matrix  \n",
    "        4. __R and __t for rotation and translation\n",
    "        \n",
    "        '''\n",
    "        self.__P = np.empty([self.__point_num, 12], dtype=float)\n",
    "        self.__roM = np.empty([3, 4], dtype=float)\n",
    "        self.__A = np.empty([3, 3], dtype=float)\n",
    "        self.__b = np.empty([3, 1], dtype=float)\n",
    "        self.__K = np.empty([3, 3], dtype=float)\n",
    "        self.__R = np.empty([3, 3], dtype=float)\n",
    "        self.__t = np.empty([3, 1], dtype=float)\n",
    "\n",
    "    def returnAb(self):\n",
    "        return self.__A, self.__b\n",
    "\n",
    "    def returnKRT(self):\n",
    "        return self.__K, self.__R, self.__t\n",
    "\n",
    "    def returnM(self):\n",
    "        return self.__roM\n",
    "\n",
    "    def myReadFile(filePath):\n",
    "        pass\n",
    "\n",
    "    def changeHomo(no_homo):\n",
    "        pass\n",
    "\n",
    "    # to compose P in right form s.t. we can get Pm=0\n",
    "    def composeP(self):\n",
    "        i = 0\n",
    "        P = np.empty([self.__point_num, 12], dtype=float)\n",
    "        # print(P.shape)\n",
    "        while i < self.__point_num:\n",
    "            c = i // 2\n",
    "            p1 = self.__world_coor[c]\n",
    "            p2 = np.array([0, 0, 0, 0])\n",
    "            if i % 2 == 0:\n",
    "                p3 = -p1 * self.__pixel_coor[c][0]\n",
    "                #print(p3)\n",
    "                P[i] = np.hstack((p1, p2, p3))\n",
    "\n",
    "            elif i % 2 == 1:\n",
    "                p3 = -p1 * self.__pixel_coor[c][1]\n",
    "                #print(p3)\n",
    "                P[i] = np.hstack((p2, p1, p3))\n",
    "            # M = P[i]\n",
    "            # print(M)\n",
    "            i = i + 1\n",
    "        print(\"Now P is with form of :\")\n",
    "        print(P)\n",
    "        print('\\n')\n",
    "        self.__P = P\n",
    "\n",
    "    # svd to P，return A,b, where M=[A b]\n",
    "    def svdP(self):\n",
    "        U, sigma, VT = LA.svd(self.__P)\n",
    "        # print(VT.shape)\n",
    "        V = np.transpose(VT)\n",
    "        preM = V[:, -1]\n",
    "        roM = preM.reshape(3, 4)\n",
    "        print(\"some scalar multiple of M,recorded as roM:\")\n",
    "        print(roM)\n",
    "        print('\\n')\n",
    "        A = roM[0:3, 0:3].copy()\n",
    "        b = roM[0:3, 3:4].copy()\n",
    "        print(\"M can be written in form of [A b], where A is 3x3 and b is 3x1, as following:\")\n",
    "        print(A)\n",
    "        print(b)\n",
    "        print('\\n')\n",
    "        self.__roM = roM\n",
    "        self.__A = A\n",
    "        self.__b = b\n",
    "\n",
    "    # solve the intrinsics and extrisics\n",
    "    def workInAndOut(self):\n",
    "        # compute ro, where ro=1/|a3|, ro may be positive or negative,\n",
    "        # we choose the positive ro and name it ro01\n",
    "        a3T = self.__A[2]\n",
    "        # print(a3T)\n",
    "        under = LA.norm(a3T)\n",
    "        # print(under)\n",
    "        ro01 = 1 / under\n",
    "        print(\"The ro is %f \\n\" % ro01)\n",
    "\n",
    "        # comput cx and cy\n",
    "        a1T = self.__A[0]\n",
    "        a2T = self.__A[1]\n",
    "        cx = ro01 * ro01 * (np.dot(a1T, a3T))\n",
    "        cy = ro01 * ro01 * (np.dot(a2T, a3T))\n",
    "        print(\"cx=%f,cy=%f \\n\" % (cx, cy))\n",
    "\n",
    "        # compute theta\n",
    "        a_cross13 = np.cross(a1T, a3T)\n",
    "        a_cross23 = np.cross(a2T, a3T)\n",
    "        theta = np.arccos((-1) * np.dot(a_cross13, a_cross23) / (LA.norm(a_cross13) * LA.norm(a_cross23)))\n",
    "        print(\"theta is: %f \\n\" % theta)\n",
    "\n",
    "        # compute alpha and beta\n",
    "        alpha = ro01 * ro01 * LA.norm(a_cross13) * np.sin(theta)\n",
    "        beta = ro01 * ro01 * LA.norm(a_cross23) * np.sin(theta)\n",
    "        print(\"alpha:%f, beta:%f \\n\" % (alpha,beta))\n",
    "\n",
    "        # compute K\n",
    "        K = np.array([alpha, -alpha * (1 / np.tan(theta)), cx, 0, beta / (np.sin(theta)), cy, 0, 0, 1])\n",
    "        K = K.reshape(3, 3)\n",
    "        print(\"We can get K accordingly: \")\n",
    "        print(K)\n",
    "        print('\\n')\n",
    "        self.__K = K\n",
    "\n",
    "        # compute R\n",
    "        r1 = a_cross23 / LA.norm(a_cross23)\n",
    "        r301 = ro01 * a3T\n",
    "        r2 = np.cross(r301, r1)\n",
    "        #print(r1, r2, r301)\n",
    "        R = np.hstack((r1, r2, r301))\n",
    "        R = R.reshape(3,3)\n",
    "        print(\"we can get R:\")\n",
    "        print(R)\n",
    "        print('\\n')\n",
    "        self.__R = R\n",
    "\n",
    "        # compute T\n",
    "        T = ro01 * np.dot(LA.inv(K), self.__b)\n",
    "        print(\"we can get t:\")\n",
    "        print(T)\n",
    "        print('\\n')\n",
    "        self.__t = T\n",
    "\n",
    "    def selfcheck(self, w_check, c_check):\n",
    "        my_size = c_check.shape[0]\n",
    "        my_err = np.empty([my_size])\n",
    "        for i in range(my_size) :\n",
    "            test_pix = np.dot(self.__roM, w_check[i])\n",
    "            u = test_pix[0] / test_pix[2]\n",
    "            v = test_pix[1] / test_pix[2]\n",
    "            u_c = c_check[i][0]\n",
    "            v_c = c_check[i][1]\n",
    "            print(\"you get test point %d with result (%f,%f)\" % (i, u, v))\n",
    "            print(\"the correct result is (%f,%f)\" % (u_c,v_c))\n",
    "            my_err[i] = (abs(u-u_c)/u_c+abs(v-v_c)/v_c)/2\n",
    "        average_err = my_err.sum()/my_size\n",
    "        print(\"The average error is %f ,\" % average_err)\n",
    "        if average_err > 0.1:\n",
    "            print(\"which is more than 0.1\")\n",
    "        else:\n",
    "            print(\"which is smaller than 0.1, the M is acceptable\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 结果测试"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **导入相关模块**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import linalg as LA"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **写入所有点的世界坐标，像素坐标，设置测试点**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The homogeneous world coodinate\n",
    "\n",
    "# Although it would be more appropriate to write a function to read the coordinates, \n",
    "# we've simplified it by listing the coordinates directly in array.\n",
    "\n",
    "# world corrdinate\n",
    "# points: (8, 0, 9), (8, 0, 1), (6, 0, 1), (6, 0, 9)\n",
    "w_xz = np.array([8, 0, 9, 1, 8, 0, 1, 1, 6, 0, 1, 1, 6, 0, 9, 1])\n",
    "w_xz = w_xz.reshape(4, 4)\n",
    "# points: (5, 1, 0), (5, 9, 0), (4, 9, 0), (4, 1, 0)\n",
    "w_xy = np.array([5, 1, 0, 1, 5, 9, 0, 1, 4, 9, 0, 1, 4, 1, 0, 1])\n",
    "w_xy = w_xy.reshape(4, 4)\n",
    "# points: (0, 4, 7), (0, 4, 3), (0, 8, 3), (0, 8, 7)\n",
    "w_yz = np.array([0, 4, 7, 1, 0, 4, 3, 1, 0, 8, 3, 1, 0, 8, 7, 1])\n",
    "w_yz = w_yz.reshape(4, 4)\n",
    "w_coor = np.vstack((w_xz, w_xy, w_yz))\n",
    "#print(w_coor)\n",
    "# pixel coordinate\n",
    "c_xz = np.array([275, 142, 312, 454, 382, 436, 357, 134])\n",
    "c_xz = c_xz.reshape(4, 2)\n",
    "c_xy = np.array([432, 473, 612, 623, 647, 606, 464, 465])\n",
    "c_xy = c_xy.reshape(4, 2)\n",
    "c_yz = np.array([654, 216, 644, 368, 761, 420, 781, 246])\n",
    "c_yz = c_yz.reshape(4, 2)\n",
    "c_coor = np.vstack((c_xz, c_xy, c_yz))\n",
    "#print(c_coor)\n",
    "# coordinate for validation whether the M is correct or not\n",
    "w_check = np.array([6, 0, 5, 1, 3, 3, 0, 1, 0, 4, 0, 1, 0, 4, 4, 1, 0, 0, 7, 1])\n",
    "w_check = w_check.reshape(5, 4)\n",
    "c_check = np.array([369, 297, 531, 484, 640, 468, 646, 333, 556, 194])\n",
    "c_check = c_check.reshape(5, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "- **计算相机参数，测试投影矩阵**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Now P is with form of :\n",
      "[[ 8.000e+00  0.000e+00  9.000e+00  1.000e+00  0.000e+00  0.000e+00\n",
      "   0.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00]\n",
      " [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  8.000e+00  0.000e+00\n",
      "   9.000e+00  1.000e+00  0.000e+00  0.000e+00  0.000e+00  0.000e+00]\n",
      " [ 8.000e+00  0.000e+00  1.000e+00  1.000e+00  0.000e+00  0.000e+00\n",
      "   0.000e+00  0.000e+00 -2.496e+03  0.000e+00 -3.120e+02 -3.120e+02]\n",
      " [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  8.000e+00  0.000e+00\n",
      "   1.000e+00  1.000e+00 -3.632e+03  0.000e+00 -4.540e+02 -4.540e+02]\n",
      " [ 6.000e+00  0.000e+00  1.000e+00  1.000e+00  0.000e+00  0.000e+00\n",
      "   0.000e+00  0.000e+00 -2.292e+03  0.000e+00 -3.820e+02 -3.820e+02]\n",
      " [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  6.000e+00  0.000e+00\n",
      "   1.000e+00  1.000e+00 -2.616e+03  0.000e+00 -4.360e+02 -4.360e+02]\n",
      " [ 6.000e+00  0.000e+00  9.000e+00  1.000e+00  0.000e+00  0.000e+00\n",
      "   0.000e+00  0.000e+00 -2.142e+03  0.000e+00 -3.213e+03 -3.570e+02]\n",
      " [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  6.000e+00  0.000e+00\n",
      "   9.000e+00  1.000e+00 -8.040e+02  0.000e+00 -1.206e+03 -1.340e+02]\n",
      " [ 5.000e+00  1.000e+00  0.000e+00  1.000e+00  0.000e+00  0.000e+00\n",
      "   0.000e+00  0.000e+00 -2.160e+03 -4.320e+02  0.000e+00 -4.320e+02]\n",
      " [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  5.000e+00  1.000e+00\n",
      "   0.000e+00  1.000e+00 -2.365e+03 -4.730e+02  0.000e+00 -4.730e+02]\n",
      " [ 5.000e+00  9.000e+00  0.000e+00  1.000e+00  0.000e+00  0.000e+00\n",
      "   0.000e+00  0.000e+00 -3.060e+03 -5.508e+03  0.000e+00 -6.120e+02]\n",
      " [ 0.000e+00  0.000e+00  0.000e+00  0.000e+00  5.000e+00  9.000e+00\n",
      "   0.000e+00  1.000e+00 -3.115e+03 -5.607e+03  0.000e+00 -6.230e+02]]\n",
      "\n",
      "\n",
      "some scalar multiple of M,recorded as roM:\n",
      "[[ 5.40654233e-02 -5.48011712e-02  3.61180175e-02 -7.58221552e-01]\n",
      " [ 2.11829207e-02 -5.15006002e-02  5.25908724e-02 -6.41872412e-01]\n",
      " [ 6.18927969e-05 -4.42791434e-05  9.30771716e-05 -1.51459919e-03]]\n",
      "\n",
      "\n",
      "M can be written in form of [A b], where A is 3x3 and b is 3x1, as following:\n",
      "[[ 5.40654233e-02 -5.48011712e-02  3.61180175e-02]\n",
      " [ 2.11829207e-02 -5.15006002e-02  5.25908724e-02]\n",
      " [ 6.18927969e-05 -4.42791434e-05  9.30771716e-05]]\n",
      "[[-0.75822155]\n",
      " [-0.64187241]\n",
      " [-0.0015146 ]]\n",
      "\n",
      "\n",
      "The ro is 8317.544777 \n",
      "\n",
      "cx=631.943866,cy=587.108010 \n",
      "\n",
      "theta is: 2.030709 \n",
      "\n",
      "alpha:284.615004, beta:221.645270 \n",
      "\n",
      "We can get K accordingly: \n",
      "[[284.61500426 140.98127115 631.94386599]\n",
      " [  0.         247.34678466 587.10801031]\n",
      " [  0.           0.           1.        ]]\n",
      "\n",
      "\n",
      "we can get R:\n",
      "[[-0.68940554  0.35894597  0.62918819]\n",
      " [-0.50961256 -0.85762317 -0.06911977]\n",
      " [ 0.51479611 -0.36829376  0.77417354]]\n",
      "\n",
      "\n",
      "we can get t:\n",
      "[[  1.69296063]\n",
      " [  8.31801962]\n",
      " [-12.59774659]]\n",
      "\n",
      "\n",
      "you get test point 0 with result (373.587780,371.495304)\n",
      "the correct result is (369.000000,297.000000)\n",
      "you get test point 1 with result (520.215162,501.331503)\n",
      "the correct result is (531.000000,484.000000)\n",
      "you get test point 2 with result (577.772139,501.192239)\n",
      "the correct result is (640.000000,468.000000)\n",
      "you get test point 3 with result (631.309457,483.180160)\n",
      "the correct result is (646.000000,333.000000)\n",
      "you get test point 4 with result (585.586194,317.169867)\n",
      "the correct result is (556.000000,194.000000)\n",
      "The average error is 0.164937 ,\n",
      "which is more than 0.1\n"
     ]
    }
   ],
   "source": [
    "aCamera = SingleCamera(w_coor, c_coor, 12)  # 12 points in total are used\n",
    "aCamera.composeP()\n",
    "aCamera.svdP()\n",
    "aCamera.workInAndOut()  # print computed result\n",
    "aCamera.selfcheck(w_check,c_check)  # test 5 points and verify M"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "2fd6ff00ff4a419d324d5b7e4b1b0789b8ee895e93e15d812a34184c59464f6c"
  },
  "kernelspec": {
   "display_name": "Python 3.8.11 64-bit ('MyCV': conda)",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.11"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
